#!/usr/bin/env python3

import rospy
import math
import yaml
import os
from std_msgs.msg import Int32, String
from nav_msgs.msg import Odometry
from nav_to_point import NavToPoint

class WaypointsDirectNavigator:
    def __init__(self):
        rospy.init_node('waypoints_direct_navigator')
        
        # 创建直接控制导航器实例
        self.navigator = NavToPoint()
        
        # 参数初始化
        self.waypoints = []
        self.current_waypoint_index = 0
        self.is_navigating = False
        self.auto_continue = True
        
        # 加载路径点
        waypoints_file = rospy.get_param('~waypoints_file', '')
        if waypoints_file:
            self.load_waypoints(waypoints_file)
        
        # 发布器和订阅器
        self.status_pub = rospy.Publisher('waypoints_status', Int32, queue_size=1)
        self.next_waypoint_sub = rospy.Subscriber('next_waypoint', String, self.next_waypoint_callback)
        self.stop_sub = rospy.Subscriber('stop_navigation', String, self.stop_navigation_callback)
        
        rospy.loginfo("路径点直接导航器初始化完成")
    
    def load_waypoints(self, file_path):
        """从YAML文件加载路径点"""
        try:
            if not os.path.isabs(file_path):
                # 如果不是绝对路径，尝试在ROS包中找到它
                pkg_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
                file_path = os.path.join(pkg_path, file_path)
            
            with open(file_path, 'r') as file:
                data = yaml.safe_load(file)
                self.waypoints = data.get('waypoints', [])
                rospy.loginfo(f"已加载 {len(self.waypoints)} 个路径点")
                
        except Exception as e:
            rospy.logerr(f"加载路径点文件失败: {e}")
    
    def next_waypoint_callback(self, msg):
        """处理下一个路径点请求"""
        if msg.data == "next":
            self.navigate_to_next_waypoint()
        elif msg.data == "auto":
            self.auto_continue = True
            self.navigate_to_next_waypoint()
        elif msg.data == "manual":
            self.auto_continue = False
    
    def stop_navigation_callback(self, msg):
        """处理停止导航请求"""
        self.is_navigating = False
        self.navigator.clean_up()
        rospy.loginfo("导航已停止")
    
    def navigate_to_next_waypoint(self):
        """导航到下一个路径点"""
        if self.current_waypoint_index >= len(self.waypoints):
            rospy.loginfo("已完成所有路径点导航")
            self.current_waypoint_index = 0
            self.status_pub.publish(Int32(-1))  # -1表示完成所有路径点
            return
        
        waypoint = self.waypoints[self.current_waypoint_index]
        self.is_navigating = True
        
        rospy.loginfo(f"正在导航到路径点 {self.current_waypoint_index + 1}/{len(self.waypoints)}: {waypoint}")
        self.status_pub.publish(Int32(self.current_waypoint_index))
        
        # 提取路径点数据
        x = waypoint.get('x', 0.0)
        y = waypoint.get('y', 0.0)
        yaw_deg = waypoint.get('yaw', 0.0)
        
        # 使用direct_navigator导航到目标点
        success = self.navigator.goto([x, y, yaw_deg])
        
        if success:
            rospy.loginfo(f"成功到达路径点 {self.current_waypoint_index + 1}")
            self.current_waypoint_index += 1
            
            # 如果设置为自动继续，则导航到下一个点
            if self.auto_continue:
                rospy.sleep(1.0)  # 在继续前暂停一下
                self.navigate_to_next_waypoint()
        else:
            rospy.logwarn(f"导航到路径点 {self.current_waypoint_index + 1} 失败")
    
    def run(self):
        """运行导航器"""
        rospy.loginfo("路径点直接导航器已启动，等待指令...")
        rospy.spin()

if __name__ == '__main__':
    try:
        navigator = WaypointsDirectNavigator()
        navigator.run()
    except rospy.ROSInterruptException:
        rospy.loginfo("路径点直接导航器已终止")
